#include<Algorithm/convex_opt.h>


/*
solve Ax=b

Fx<=f
梯度下降法
*/
/*
delta > 0

*/

static void  compute_matrix_mult_matrix(
    double** A,int row1,int col1,double **B,int col2,
    double** re)
{
    for(int i=0;i<row1;i++)
    {
        for(int j=0;j<col2;j++)
        {
            double temp=0;
            for(int k=0;k<col1;k++)
            {
                temp+=A[i][k]*B[k][j];
            }
            re[i][j]=temp;
        }
    }
}

static void  compute_ATA(
    double** A,int row,int col,
    double** re)
{
    for(int i=0;i<col;i++)
    {
        for(int j=0;j<col;j++)
        {
            double temp=0;
            for(int k=0;k<row;k++)
            {
                temp+=A[k][i]*A[k][j];
            }
            re[i][j]=temp;
        }

    }

}

static void  compute_AAT(
    double** A,int row,int col,
    double** re)
{
    for(int i=0;i<row;i++)
    {
        for(int j=0;j<row;j++)
        {
            double temp=0;
            for(int k=0;k<col;k++)
            {
                temp+=A[i][k]*A[j][k];
            }
            re[i][j]=temp;
        }
    }
}

static void matrix_mult_array(double** A,
    int row,int col,double*array,double*re)
{
    for(int i=0;i<row;i++)
    {
        // double temp=0;
        re[i]=0;
        for(int j=0;j<col;j++)
        {
            re[i]+=A[i][j]*array[j];
        }

    }
}

static double compute_error_gdse(
    double** A,int row,int col,double *x ,double*b)
{
    double re=0;
    for(int i=0;i<row;i++)
    {
        double temp=0;
        for(int j=0;j<col;j++)
        {
            temp+=A[i][j]*x[j];
        }
        temp-=b[i];
        re+=(temp*temp);
    }
    return re;
}

/*
solve Ax=b


梯度下降法

Fx<=f
*/
/*
delta > 0

*/

static void handle_inequality_constraint(double*x,double* tx,int col,
    double**F,double* f,int len_f )
{
    if(len_f<=0||F==NULL||f==NULL)
    {
        return;
    }
    for(int j=0;j<len_f;j++)
    {
        double temp_d=-f[j];
        for(int k=0;k<col;k++)
        {
            // fx[j]-=F[j][k]*x[k];
            temp_d+=F[j][k]*(tx[k]+x[k]);
        }
        if(temp_d>0)
        {
            double inn=inner_product(F[j],F[j],col);
            for(int k=0;k<col;k++)
            {
                tx[k]=tx[k]- (temp_d/inn)*F[j][k];
            }
        }
    }
}

double gradient_descent_for_solving_systems_of_equations(
    double **A,double*b,double*x,double**F,double *f,int len_f,
    int row,int col,double delta,int num)
{
    // int num=100;

    double** ATA=(double**)malloc(sizeof(double*)*col);
    double *ATb=(double*)malloc(sizeof(double)*col);
    double *tx=(double*)malloc(sizeof(double)*col);

    for(int i=0;i<col;i++)
    {
        ATA[i]=(double*)malloc(sizeof(double)*col);
        ATb[i]=0;
        for(int j=0;j<row;j++)
        {
            ATb[i]+=A[j][i]*b[j];
        }
    }

    compute_ATA(A,row,col,ATA);
    // double* fx=(double*)malloc(sizeof(double)*len_f);
    // for(int i=0;i<)
    // {

    // }
// 此处用了动量法，加速收敛
    //
    double momentum_coe=0.6;

    for(int i=0;i<num;i++)
    {
        double erro=compute_error_gdse(A,row,col,x ,b);
        printf("%d :erro:%.10lf\n",i,erro);
        if(fabs(erro)<1e-11 ){break;}

        for(int j=0;j<col;j++)
        {
            double tempd=0;
            for(int k=0;k<col;k++)
            {
                tempd+=ATA[j][k]*x[k] ;
            }
            tx[j]= tx[j]*momentum_coe-delta*(tempd-ATb[j]);

        }
        handle_inequality_constraint(x,tx,col,
            F,f,len_f );


        for(int j=0;j<col;j++)
        {
            x[j]=x[j] +tx[j];
        }

    }


    for(int i=0;i<col;i++)
    {
        free(ATA[i]);
    }
    free(ATA);
    free(ATb);free(tx);
    // free(fx);
    // matrix_mult_array(A,row,col,x,tx);

    return compute_error_gdse(A,row,col,x ,b);
}

/*
min 0.5* x^t H x+ C^t x

Ax<=b
Ex=d

*/

void opt_quadratic_problem_with_constraint(double**H,double* C,int len ,
    double** A,double*b ,int len_a,
    double** E,double* d,int len_e,double* x)
{
    //
    double test_t[2]={0};
    test_t[1]=0.850574;test_t[0]=1-test_t[1];

    // 0.850574
    //
    double testp[3]={-0.1,-2.3,8.9};
    double tsum=0;
    for(int i=0;i<len;i++)
    {
        for(int j=0;j<len;j++)
        {
            tsum+=(H[i][j]*test_t[i]*test_t[j]*0.5);
        }
        tsum+=C[i]*test_t[i];
    }
    printf("dis:%lf\n", SAFE_SQRT(inner_product(testp,testp,3)+tsum) );


    double ** GA=(double**)malloc(sizeof(double*)*(len+len_e));
    double* gb=(double*)malloc(sizeof(double)*(len+len_e));
    memset(gb,0,sizeof(double)*(len+len_e));

    for(int i=0;i<len+len_e;i++)
    {
        GA[i]=(double*)malloc(sizeof(double)*(len+len_e));
        memset(GA[i],0,sizeof(double)*(len+len_e));
    }


    for(int i=0;i<len;i++)
    {
        memmove(GA[i],H[i],sizeof(double)*len);
        for(int j=0;j<len_e;j++)
        {
            GA[i][j+len]=E[j][i];
            GA[j+len][i]=GA[i][j+len];
        }
    }
    for(int i=0;i<len;i++)
    {
        gb[i]=-C[i];
    }
    // memmove(gb,C,sizeof(double)*len);
    for(int i=0;i<len_e;i++)
    {
        gb[len+i]=d[i];
    }


    double * gx=(double*)malloc(sizeof(double)*(len+len_e));

    double** F=NULL;
    if(len_a>0)
    {
        F=(double**)malloc(sizeof(double*)*len_a);

        for(int i=0;i<len_a;i++)
        {
            F[i]=(double*)malloc(sizeof(double)*(len+len_e));
            memset(F[i],0,sizeof(double)*(len+len_e));
            memmove(F[i],A[i],sizeof(double)*len);
        }
    }


    double error= gradient_descent_for_solving_systems_of_equations(
        GA,gb,gx,F ,b,len_a,
        len+len_e,len+len_e,0.1,60);

    printf("gx:%lf %lf\n",gx[0],gx[1]);

    printf("error :%lf\n",error );
    memmove(x,gx,sizeof(double)*len);

    free(gx);

    if(len_a>0)
    {
        for(int i=0;i<len_a;i++)
        {
            free(F[i]);
        }
        free(F);
    }
    for(int i=0;i<len+len_e;i++)
    {
        free(GA[i]);
    }
    free(GA);free(gb);

}
